/*
 ********************************************************************************
 *                      桂林聚联科技有限公司
 *                           Copyright
 *
 *  文件描述 ： OTDR算法相关函数
 *
 *
 *  文件名称 ： OtdrAlgo.c
 *  创建者   ： 彭怀敏
 *  创建日期 ： 2016-10-26 20:37:45
 *  当前版本 ： 
 ********************************************************************************
*/
#include <stdio.h>
#include <math.h>
#include <semaphore.h>
#include <unistd.h>

#include "Otdr.h"
#include "prototypes.h"
#include "OtdrSpiData.h"
#include "DspFpgaReg.h"
#include "fpga-2016.h"

/*
 **************************************************************************************************
 *  函数  名： RemoveBaseLine
 *  函数描述： remove average value of input data
 *  入口参数： 
 *  返回参数： 
 *  日期版本： 2016年 10月 20日 星期四 17:41:36 CST
 **************************************************************************************************
*/
void RemoveBaseLine(Int32 input[], Int32 DataLen, Int32 NoiseLen)
{
    Int32 i, st, avg, *An = input;

    st = DataLen-NoiseLen;
    for(i = st; i < st+16; i++)
    {
        An[i] = An[i+16];
    }
    
	avg   = NoiseMean(An, DataLen, NoiseLen);
    for(i = 0; i < DataLen; i++)
    {
    	An[i] -= avg;
    }
}

/*
 ********************************************************************************
 *  函数名称 ： DeleteOsc
 *  函数描述 ： 将平顶拉平
 *  入口参数 ： 
 *  返回参数 ： 
 *  日期版本 ： 2016-10-24 16:24:53
 ********************************************************************************
*/
void DeleteOsc(Int32 input[], Int32 DataLen, OtdrStateCtrl_t *StateCtrl)
{
    Int32 i, j, t, interval = StateCtrl->M;
    Int32 peakValue, osc, sat = StateCtrl->SatThreshold;
    Int32 SatStartPos, SatEndPos;
    
/************************************** 将平顶处的振荡补平 ***************************************/
    if(sat == 0)    return;

    m1_print("smoothing sat");
    i = 1;
    while(i < DataLen)
    {
        if(input[i] >= sat)     // 搜索到平顶饱和信号
        {
            SatStartPos = i;
            SatEndPos   = DataLen+1;
            
            // 寻找下一个不饱和值
            for(j = i+1; j < DataLen-interval; j++)
            {
                if(input[j] < sat)	// 2015-12-22 落在平顶阈值之外，同时还要考察其后一定的M点距离内是否仍然存在平顶
                {
					osc = 0;    // 标记是否仍然存在饱和值
					for(t = j+1; t < j+interval; t++)
                    {
						if(input[t] >= sat){
							osc = 1;
							j = t;
							break;
						}
                    }
					
					if(osc == 0)	// 不再存在平顶震荡，退出搜索，否则继续搜索
                    {
						SatEndPos = j-1;
						i = j;
						break;
					}
                }
            }
            
            // 2016年 12月 28日 星期三 18:02:31 CST
            if(SatEndPos == DataLen+1){
                m1_error("SatEndPos == DataLen+1\n");
                break;
            }
            else if(SatStartPos < SatEndPos){
                // 看看该曲线的平顶值是多少，也就是最大值，同时让饱和区全部等于最大值
                MaxValue(input, SatStartPos, SatEndPos, &peakValue, NULL, DATA_TYPE_INT);
                m1_print(" : %d ~ %d", SatStartPos, SatEndPos);
                for(j = SatStartPos; j <= SatEndPos; j++)       input[j] = peakValue;
            }
        }
        else    i++;
    }
    m1_print("\n");
}

/*
 **************************************************************************************************
 *  函数  名： EnlargeData
 *  函数描述： multipy an integer to enlarge the data
 *  入口参数： 
 *  返回参数： 
 *  日期版本： 2016年 10月 20日 星期四 17:41:20 CST
 **************************************************************************************************
*/
void EnlargeData(Int32 input[], Int32 DataLen, Int32 measuretime)
{
    Int32 i, ratio;
    
    ratio = ENLARGE_FACTOR(measuretime);
    ratio = MAX(ratio, 1);   // 放大因子至少为1

    for(i = 0; i < DataLen; i++)
    {
    	input[i] *= ratio;
    }
}

/*
 **************************************************************************************************
 *  函数  名： AdjustCurve
 *  函数描述： remove redundant data at the front of input
 *  入口参数： 
 *  返回参数： 
 *  日期版本： 2016年 10月 20日 星期四 17:57:30 CST
 **************************************************************************************************
*/
void AdjustCurve(Int32 input[], Int32 DataLen, int rm)
{
    Int32 i;
    for(i = 0; i < DataLen - rm; i++)
    {
    	input[i] = input[i+rm];
    }
}

/*
 ********************************************************************************
 *  函数名称 ： NoiseMean
 *  函数描述 ： 计算噪声平均值
 *  入口参数 ： 
 *  返回参数 ： 
 *  日期版本 ： 2016-10-26 20:38:31
 ********************************************************************************
*/
Int32 NoiseMean(const Int32 input[], Int32 DataLen, Int32 NoiseLen)
{
	Int32 i;
	const Int32 *p;
	double sum = 0;
	
	p = &input[DataLen-1];
	for(i = 0; i < NoiseLen; i++)
		sum += *p--;
	
	return (Int32)(sum / NoiseLen);
}

/*
 **************************************************************************************************
 *  函数  名： RootMeanSquare
 *  函数描述： calculate the root mean square of the input data
 *  入口参数： 
 *  返回参数： 
 *  日期版本： 2016年 10月 21日 星期五 08:58:29 CST
 **************************************************************************************************
*/
Int32 RootMeanSquare(const Int32 input[], Int32 DataLen, Int32 NoiseLen)
{
	Int32 i, sigma;
	const Int32 *p;
	double v, sum = 0;
	
	p = &input[DataLen-1];
	for(i = 0; i < NoiseLen; i++)
	{
		v = (double)(*p--);
		sum += v * v;
	}
	sigma = (Int32)sqrt(sum / NoiseLen);
	sigma = MAX(sigma, 1);
	return sigma;
}

/*
 ********************************************************************************
 *  函数名称 ： PulseWidthInSampleNum
 *  函数描述 ： 
 *  入口参数 ： 
 *  返回参数 ： 
 *  日期版本 ： 2016-10-24 16:23:31
 ********************************************************************************
*/
Int32 PulseWidthInSampleNum(OtdrStateCtrl_t *StateCtrl)
{
    Int32 m;
    m = (Int32)(StateCtrl->MeasureParam.PulseWidth_ns * (StateCtrl->RealSampleRate_Hz / 1000) / 1e6);

    m = MAX(m, 2);
    return m;
}

/*
 **************************************************************************************************
 *  函数  名： GetSaturateThreshold
 *  函数描述： get the saturate threshold of otdr data
 *  入口参数： 
 *  返回参数： 
 *  日期版本： 2016年 10月 20日 星期四 18:01:02 CST
 **************************************************************************************************
*/
Int32 GetSaturateThreshold(const Int32 input[], OtdrStateCtrl_t *StateCtrl)
{
    Int32 i, count, maxcount, temp, SatThreshold;
    Int32 DataNum, yes = 0;
    
    DataNum = StateCtrl->DataNum;
    MaxValue(input, 0, DataNum-NOISE_NUM-StateCtrl->M, &temp, &i, DATA_TYPE_INT);

    SatThreshold = temp * Tenth_div5(-0.05);
    m1_print("GetSaturateThreshold : MaxValue at %d is %d, low down to %d\n", i, temp, SatThreshold);
    
    count = 0;
    maxcount = MIN(StateCtrl->M, 100);
    maxcount = MAX(maxcount, 5);
    for(i = 0; i < DataNum-NOISE_NUM-StateCtrl->M; i++)
    {
        if(input[i] >= SatThreshold)
        {
            count++;
            if(count >= maxcount)     break;
        }
    }
    
    if(count >= maxcount)
    {
        m1_print("There's saturate point from %d\n", i);
        yes = 1;
    }
    
    if(!yes)     SatThreshold *= 1.5;
    StateCtrl->SatThreshold = SatThreshold;
    return yes;
}

/*
 ********************************************************************************
 *  函数名称 ： GetMeasureLengthIndex GetPulseWidthIndex GetLambdaIndex GetPowerLevelIndex
 *  函数描述 ： 
 *  入口参数 ： 
 *  返回参数 ： 
 *  日期版本 ： 2016-10-26 17:52:21
 ********************************************************************************
*/
int GetMeasureLengthIndex(int MeasureLength_m)
{
    int index;
    
    for(index = MEASURE_LENGTH_NUM-1; index >= 0; index--)
    {
        if(MeasureLength_m == (int)OtdrMeasureLength[index])    break;
    }
    return index;
}

int GetPulseWidthIndex(int PulseWidth_ns)
{
    int index;
    
    if(PulseWidth_ns <= 5)                  index = 0;
    else if(PulseWidth_ns <= 10)            index = 1;
    else if(PulseWidth_ns <= 20)            index = 2;
    else if(PulseWidth_ns <= 40)            index = 3;
    else if(PulseWidth_ns <= 80)            index = 4;
    else if(PulseWidth_ns <= 160)           index = 5;
    else if(PulseWidth_ns <= 320)           index = 6;
    else if(PulseWidth_ns <= 640)           index = 7;
    else if(PulseWidth_ns <= 1280)          index = 8;
    else if(PulseWidth_ns <= 2560)          index = 9;
    else if(PulseWidth_ns <= 5120)          index = 10;
    else if(PulseWidth_ns <= 10240)         index = 11;
    else /*if(PulseWidth_ns <= 20480)*/     index = 12;
    
    return index;
}

Uint32 GetLambdaIndex(Uint32 Lambda)
{
    Uint32 i, j = 1;
    
    for(i = 0; i < LAMBDA_NUM; i++)
    {
        if(OtdrLambdaIndex[i] == Lambda)
        {
            j = i;
            break;
        }
    }
    
    return j;
}

OtdrPowerLevel_t GetPowerLevelIndex(Uint32 Lambda)
{
    OtdrPowerLevel_t pl;
    
    if(Lambda == 1310)          pl = OtdrPowerLevel1310;
    else if(Lambda == 1625)     pl = OtdrPowerLevel1625;
    else if(Lambda == 1650)     pl = OtdrPowerLevel1625;
    else                        pl = OtdrPowerLevel1550;
    
    return pl;
}

OtdrPowerLevelLP_t GetPowerLevelLPIndex(Uint32 Lambda)
{
    OtdrPowerLevelLP_t pl;
    
    if(Lambda == 1310)          pl = OtdrPowerLevel1310LP;
    else if(Lambda == 1625)     pl = OtdrPowerLevel1625LP;
    else if(Lambda == 1650)     pl = OtdrPowerLevel1625LP;
    else                        pl = OtdrPowerLevel1550LP;
    
    return pl;
}

/*
 ********************************************************************************
 *  函数名称： GetPulseWidthSaturateBlindZone
 *  函数描述： 
 *  入口参数： 
 *  返回参数： 
 *  日期版本： 2016-10-21 17:23:50
 ********************************************************************************
*/
Uint32 GetPulseWidthSaturateBlindZone(Uint32 Lambda_nm, Uint32 PulseWidth_ns)
{
    Int32 a, b, i, Cn, st;

    i = GetLambdaIndex(Lambda_nm);
    i = MIN(i, 1);
    
    PulseWidth_ns = MIN(PulseWidth_ns, 20480);
    if((PulseWidth_ns >= 640) && (PulseWidth_ns < 1280))
        {a = OtdrBlindZone[i][0], b = OtdrBlindZone[i][1], st = 640;}
    else if((PulseWidth_ns >= 1280) && (PulseWidth_ns < 2560))
        {a = OtdrBlindZone[i][1], b = OtdrBlindZone[i][2], st = 1280;}
    else if((PulseWidth_ns >= 2560) && (PulseWidth_ns < 5120))
        {a = OtdrBlindZone[i][2], b = OtdrBlindZone[i][3], st = 2560;}
    else if((PulseWidth_ns >= 5120) && (PulseWidth_ns < 10240))
        {a = OtdrBlindZone[i][3], b = OtdrBlindZone[i][4], st = 5120;}
    else if((PulseWidth_ns >= 10240) && (PulseWidth_ns < 20480))
        {a = OtdrBlindZone[i][4], b = OtdrBlindZone[i][5], st = 10240;}
    else /* PulseWidth_ns == 20480 */
        {a = OtdrBlindZone[i][5], b = OtdrBlindZone[i][5], st = 20480;}
    
    a &= 0xffff;
    b &= 0xffff;
    Cn = a + (float)(b - a) * (PulseWidth_ns - st) / st;
    return Cn;
}

/*
 ********************************************************************************
 *  函数名称： CapacityLinearCompensate
 *  函数描述： 
 *  入口参数： 
 *  返回参数： 
 *  日期版本： 2016-10-21 09:37:10
 ********************************************************************************
*/
Int32 CapacityLinearCompensate(Int32 input[], Int32 DataLen, Int32 FilterLen, Int32 sigma, OtdrStateCtrl_t *StateCtrl)
{
    Int32   i, BadPoint;
    Int32   MinTrend, Avg;
	Int32   *TrendCurve = NULL;

/********************************************************************************/
    TrendCurve = (Int32*)malloc(DataLen * sizeof(Int32));
    if(NULL == TrendCurve)
    {
        m1_warning("CapacityLinearCompensate : Trendcurve malloc failed, DO NOTHING!!!\n");
        return E_MEM_ALLOC;
    }

    m1_print("CapacityLinearCompensate starts\n");
	nfir_center(input, TrendCurve, DataLen, FilterLen);

/******************************** find badpoint to compensate *********************************/
    if((StateCtrl->MeasureParam.PulseWidth_ns < MIN_PW_DATA_COMBINE) &&
       (StateCtrl->MeasureParam.MeasureLength_m >= 100000))
    {
        BadPoint = DataLen - NOISE_NUM;
        MinTrend = 0;
        for(i = FilterLen; i < DataLen-1-NOISE_NUM; i++)
        {
            if(TrendCurve[i] <= MinTrend)   break;  // first zero point of TrendCurve
        }
        BadPoint = i;
    }
    else
    {
        // Lowest point of TrendCurve
        MinValue(TrendCurve, FilterLen, DataLen-1 - NOISE_NUM, &MinTrend, &BadPoint, DATA_TYPE_INT);
        if(MinTrend > -sigma/2){
            free(TrendCurve);
            //m1_print("TrendCurve UNFREE!\n");
            return E_CMD_OK;
        }
    }
    if(BadPoint > DataLen-NOISE_NUM){
        free(TrendCurve);
        //m1_print("TrendCurve UNFREE!\n");
        return E_CMD_OK;
    }

	Avg = MinTrend;
    StateCtrl->SignalBadPoint = BadPoint;

/************************************** Start Compensate ***************************************/
    for(i = BadPoint; i < DataLen - NOISE_NUM; i++)
    {
        input[i] = input[i] - TrendCurve[i];
    }
    for(i = 0; i < BadPoint; i++)
    {
        input[i] -= Avg;
    }
    StateCtrl->SatThreshold -= Avg;

    free(TrendCurve);
    return 1;
}

/*
 ********************************************************************************
 *  函数名称 ： OtdrConcatCurve
 *  函数描述 ： 低功率曲线和高功率曲线进行拼接。实际上并不直接在指数域上拼接，
 *              而是输出高低功率曲线的差值，在计算对数曲线时再进行拼接
 *  入口参数 ： 
 *  返回参数 ： 
 *  日期版本 ： 2016-10-24 16:32:36
 ********************************************************************************
*/
int OtdrConcatCurve(OtdrData_t *OtdrData, OtdrStateCtrl_t *StateCtrl)
{
    Int32   m, Lavg, Havg, DataNum;
	Int32   *An, *Bn;
	float   v, LdB;
    
    An = OtdrData->lp;
    Bn = OtdrData->hp;
    DataNum = StateCtrl->DataNum;
    m1_print("OtdrConcatCurve starts\n");
/************** 如果低功率数据发生变化，则进行和高功率相同步骤的处理 *****************/
    if(StateCtrl->LowPowerDataChanged)
    {
        RemoveBaseLine(An, DataNum, NOISE_NUM);
        EnlargeData(An, DataNum, StateCtrl->LowPowerTime);
        AdjustCurve(An, DataNum, StateCtrl->CurveStartPoint);

    	m = GetNfirWidth(StateCtrl);
    	nfir(An, An, DataNum, m);
    	StateCtrl->sigmaLp = RootMeanSquare(An, DataNum, NOISE_NUM-StateCtrl->M);
    	CapacityLinearCompensate(An, DataNum, MAX(StateCtrl->M, 128), StateCtrl->sigmaLp, StateCtrl);
    }
/************************** 输出高低功率曲线在拼接点处的动态差值 **************************/
    m = StateCtrl->ConcatParam.ConcatPoint;
    MeanValue(An, m-20, m+20, &Lavg, DATA_TYPE_INT);
    MeanValue(Bn, m-20, m+20, &Havg, DATA_TYPE_INT);

    v = (float)Lavg/StateCtrl->sigmaLp;
    LdB = 5*FastLog10(v);
    if(LdB < 10){
        m1_warning("LowPowerData too low at ConcatPoint (%.2fdB at %.2fkm)\n", LdB, m/StateCtrl->Points_1m /1000);
        return E_CONCAT;
    }
    else{
        v = (float)Havg/Lavg;
        StateCtrl->HighMinusLow = 5*FastLog10(v);
        return E_CMD_OK;
    }
}

/*
 ********************************************************************************
 *  函数名称： FpgaStart    TestParam2FpgaStartParam
 *  函数描述： 
 *  入口参数： 
 *  返回参数： 
 *  日期版本： 2016-10-21 11:04:06
 ********************************************************************************
*/
static Uint16 getLaserMap(Uint32 lambda)
{
#if 0
    int i, t = 1310;
    for(i = 0; i < 6; i++)
    {
        if(lambda == OtdrDefaultLaserMap1550[i])
        {
            t = 1550;
            break;
        }
    }
    return (Uint16)t;
#else
    return lambda;
#endif
}

void TestParam2FpgaStartParam(OtdrStateCtrl_t *StateCtrl, FpgaStartParam_t *fsp, int powerlevel, int rcv, int apdv, int measuretime)
{
	Uint16 lambda, clk, sample_count;
    struct fpga_test_param *p = &fsp->ftp;

    lambda      = getLaserMap(StateCtrl->MeasureParam.Lambda_nm);
    clk         = StateCtrl->RealSampleRate_MHz;
    
    sample_count = (Uint32)(1000*measuretime/StateCtrl->PulsePeriod_us);

    fsp->sample_count = sample_count;
    fsp->measuretime = measuretime;

    p->laser_ch    = lambda;
    p->pulse    = StateCtrl->MeasureParam.PulseWidth_ns;
    p->sample_freq = clk;
    p->adopt_pt = StateCtrl->DataNum;
    p->add_cnt  = sample_count;

    p->en_hw_amp = 1;
    p->hw_amp    = rcv;

    p->en_hw_apd_vol = 1;
    p->hw_apd_vol = apdv;

    p->power    = powerlevel;
    m1_print("power = %d, rcv = 0x%02x, apdv = %d\n", powerlevel, rcv, apdv);

#if 0
    m1_print("************* fpga_test_param ****************\n");
    m1_print("        p->laser_ch    = %d\n",       p->laser_ch    );
    m1_print("        p->pulse       = %d\n",       p->pulse       );
    m1_print("        p->sample_freq = %d\n",       p->sample_freq );
    m1_print("        p->adopt_pt    = %d\n",       p->adopt_pt    );
    m1_print("        p->add_cnt     = %d\n",       p->add_cnt     );
    m1_print("        p->hw_amp(RCV) = 0x%02x\n",   p->hw_amp      );
    m1_print("        p->hw_apd_vol  = %d\n",       p->hw_apd_vol  );
    m1_print("        p->power       = %d\n",       p->power       );
#endif
}

void FpgaStart_sendTestParam(DeviceInfo_t *dev, FpgaStartParam_t *fsp)
{
    static unsigned long lastAmp = 0x7FFFFFFF;
    static unsigned char lastP = 0x7F, lastV = 0x7F;
    int st, st1 = 0, st2 = 0, st3 = 0;

    if(fsp->ftp.hw_amp != lastAmp){
        st1 = 10;
        lastAmp = fsp->ftp.hw_amp;
    }
    if(fsp->ftp.power!= lastP){
        st2 = 10;
        lastP = fsp->ftp.power;
    }
    if(fsp->ftp.hw_apd_vol != lastV){
        st3 = 20;
        lastV = fsp->ftp.hw_apd_vol;
    }
    st = MAX(st1, st2);
    st = MAX(st , st3);
    
    // send command to fpga
    sendTestParam(dev, fsp);
    //sleep(10);
    usleep(st*1000);
}

void FpgaStart(DeviceInfo_t *dev, FpgaStartParam_t *fsp)
{
    //char  timebuf[32];
    //m1_print("FpgaStart at time : %s\n", CurrentDateTime(timebuf));
    FpgaStart_sendTestParam(dev, fsp);
    sendStartCmd(dev);
}

void FpgaStop(DeviceInfo_t *dev)
{
    sendStopCmd(dev);
}

/*
 **************************************************************************************************
 *    End    of    File
 **************************************************************************************************
*/
